#include <cmath>
using namespace std;
struct Point3D
{
    double x;
    double y;
    double z;
};

extern bool cagaus(double A[],double b[], int perm[], int N, double x[]);

bool sphereLeastFit(const std::vector<Point3D> &points,
                    double &center_x, double &center_y, double &center_z, double &radius)
{
    center_x = 0.0f;
    center_y = 0.0f;
    radius = 0.0f;

    int N = points.size();
    if (N < 4) // 最少 4 个点才能拟合出一个球面。
    {
        return false;
    }

    double sum_x = 0.0f, sum_y = 0.0f, sum_z = 0.0f;

    for (int i = 0; i < N; i++)
    {
        double x = points[i].x;
        double y = points[i].y;
        double z = points[i].z;
        sum_x += x;
        sum_y += y;
        sum_z += z;
    }
    double mean_x = sum_x / N;
    double mean_y = sum_y / N;
    double mean_z = sum_z / N;
    double sum_u2 = 0.0f, sum_v2 = 0.0f, sum_w2 = 0.0f;
    double sum_u3 = 0.0f, sum_v3 = 0.0f, sum_w3 = 0.0f;
    double sum_uv = 0.0f, sum_uw = 0.0f, sum_vw = 0.0f;
    double sum_u2v = 0.0f, sum_uv2 = 0.0f, sum_u2w = 0.0f, sum_v2w = 0.0f, sum_uw2 = 0.0f, sum_vw2 = 0.0f;
    for (int i = 0; i < N; i++)
    {
        double u = points[i].x - mean_x;
        double v = points[i].y - mean_y;
        double w = points[i].z - mean_z;
        double u2 = u * u;
        double v2 = v * v;
        double w2 = w * w;
        sum_u2 += u2;
        sum_v2 += v2;
        sum_w2 += w2;
        sum_u3 += u2 * u;
        sum_v3 += v2 * v;
        sum_w3 += w2 * w;
        sum_uv += u * v;
        sum_vw += v * w;
        sum_uw += u * w;
        sum_u2v += u2 * v;
        sum_u2w += u2 * w;
        sum_uv2 += u * v2;
        sum_v2w += v2 * w;
        sum_uw2 += u * w2;
        sum_vw2 += v * w2;
    }

    double A[3][3];
    double B[3] = {(sum_u3 + sum_uv2 + sum_uw2) / 2.0,
                   (sum_u2v + sum_v3 + sum_vw2) / 2.0,
                   (sum_u2w + sum_v2w + sum_w3) / 2.0
                  };

    A[0][0] = sum_u2;
    A[0][1] = sum_uv;
    A[0][2] = sum_uw;
    A[1][0] = sum_uv;
    A[1][1] = sum_v2;
    A[1][2] = sum_vw;
    A[2][0] = sum_uw;
    A[2][1] = sum_vw;
    A[2][2] = sum_w2;

    double ans[3];int js[3];

    if( cagaus((double *) A, B, js, 3, ans) == 0)
    {
        return false;
    }
    else
    {
        center_x = ans[0] + mean_x;
        center_y = ans[1] + mean_y;
        center_z = ans[2] + mean_z;

        double sum = 0;
        for(int i = 0; i < N; i++)
        {
            double dx = points[i].x - center_x;
            double dy = points[i].y - center_y;
            double dz = points[i].z - center_z;
            sum += dx * dx + dy * dy + dz * dz;
        }
        sum /= N;
        radius = sqrt(sum);
    }
    return true;
}
